#include "S_MPU9250.h"

// #include "quaternion_supervisor.h"
//q30，q16格式,long转float时的除数.
#define q30 1073741824.0f
#define q16 65536.0f
//陀螺仪方向设置
static signed char gyro_orientation[9] = {1, 0, 0,
										  0, 1, 0,
										  0, 0, 1};
//磁力计方向设置
static signed char comp_orientation[9] = {0, 1, 0,
										  1, 0, 0,
										  0, 0, -1};
/**
 * @brief 9250自测试
 * 
 * @return 0则正常
 */
uint8_t run_self_test(void)
{
	int result;
	//char test_packet[4] = {0};
	long gyro[3], accel[3];
	result = mpu_run_6500_self_test(gyro, accel, 0);
	if (result == 0x7)
	{
		/* Test passed. We can trust the gyro data here, so let's push it down
		* to the DMP.
		*/
		unsigned short accel_sens;
		float gyro_sens;

		mpu_get_gyro_sens(&gyro_sens);
		gyro[0] = (long)(gyro[0] * gyro_sens);
		gyro[1] = (long)(gyro[1] * gyro_sens);
		gyro[2] = (long)(gyro[2] * gyro_sens);
		//inv_set_gyro_bias(gyro, 3);
		dmp_set_gyro_bias(gyro);
		mpu_get_accel_sens(&accel_sens);
		accel[0] *= accel_sens;
		accel[1] *= accel_sens;
		accel[2] *= accel_sens;
		// inv_set_accel_bias(accel, 3);
		dmp_set_accel_bias(accel);
		return 0;
	}
	else
		return 1;
}
//方向转换
uint8_t inv_row_2_scale(const signed char *row)
{
	uint8_t b;

	if (row[0] > 0)
		b = 0;
	else if (row[0] < 0)
		b = 4;
	else if (row[1] > 0)
		b = 1;
	else if (row[1] < 0)
		b = 5;
	else if (row[2] > 0)
		b = 2;
	else if (row[2] < 0)
		b = 6;
	else
		b = 7; // error
	return b;
}

static uint8_t mpu9250_readID(void)
{
	uint8_t id;
	HAL_I2C_Mem_Read(&hi2c1, MPU9250Addr, MPU9250_WHO_AM_I, 1, &id, 1, 0xff);
	return id;
}


static uint8_t mpu9250_init(void)
{
	if (mpu9250_readID() == 0x71)
	{
		uint8_t buf[6];

		uint8_t reset = 0x80;
		uint8_t wakeup = 0x00;
		uint8_t disableint = 0x00;
		uint8_t gyrocfg = 0b00010000;
		uint8_t acclcfg = 0b00001000;
		uint8_t cfg = MPU9250_DLPF_BW_20;
		uint8_t smpdiv = 0x00;
		uint8_t intpincfg = 0x02;

		uint8_t ak8963cfg = 0x11;

		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_PWR_MGMT_1, 1, &reset, 1, 0xff); //复位

		HAL_Delay(100);

		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_PWR_MGMT_1, 1, &wakeup, 1, 0xff);	 //唤醒
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_INT_ENABLE, 1, &disableint, 1, 0xff); //禁止中断
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_GYRO_CONFIG, 1, &gyrocfg, 1, 0xff);	 //陀螺仪高精度
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_ACCEL_CONFIG, 1, &acclcfg, 1, 0xff);	 //加速度计高精度
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_CONFIG, 1, &cfg, 1, 0xff);			 //设置陀螺仪的输出1Khz
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_SMPLRT_DIV, 1, &smpdiv, 1, 0xff);	 //采样分频
		HAL_I2C_Mem_Write(&hi2c1, MPU9250Addr, MPU9250_INT_PIN_CFG, 1, &intpincfg, 1, 0xff); //MPU可以直接访问磁力计

		HAL_I2C_Mem_Write(&hi2c1, AK8963Addr, 0x0A, 1, &ak8963cfg, 1, 0xff);

		return 1;
	}
	else
		return 0;
}

uint8_t MPU9250_MPL_Init(void)
{
	uint8_t res = 0;
	struct int_param_s int_param;
	uint8_t accel_fsr;
	uint16_t gyro_rate, gyro_fsr;
	uint16_t compass_fsr;

	if (!mpu_init(&int_param))
	{
		res = inv_init_mpl(); //初始化MPL
		if (res)
			return 1;
		inv_enable_quaternion();
		inv_enable_9x_sensor_fusion();
		inv_enable_fast_nomot();
		inv_enable_gyro_tc();
		inv_enable_vector_compass_cal();
		inv_enable_magnetic_disturbance();
		inv_enable_eMPL_outputs();
		res = inv_start_mpl(); //开启MPL
		if (res)
			return 1;
		mpl_printf("mpl start OK!\n");
		res = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); //设置所需要的传感器
		if (res)
			return 2;
		mpl_printf("sensor set OK!\n");
		res = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //设置FIFO
		if (res)
			return 3;
		mpl_printf("FIFO set OK!\n");
		res = mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率//宏定义在头文件COMPASS_READ_MS
		if (res)
			return 4;
		mpl_printf("Sample Rate set OK!\n");
		res = mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS); //设置磁力计采样率
		if (res)
			return 5;
		mpl_printf("Compass_Sample_Rate set OK!\n");
		mpu_get_sample_rate(&gyro_rate);
		mpu_get_gyro_fsr(&gyro_fsr);
		mpu_get_accel_fsr(&accel_fsr);
		mpu_get_compass_fsr(&compass_fsr);
		inv_set_gyro_sample_rate(1000000L / gyro_rate);
		inv_set_accel_sample_rate(1000000L / gyro_rate);
		inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L); //宏定义在头文件COMPASS_READ_MS
		inv_set_gyro_orientation_and_scale(
			inv_orientation_matrix_to_scalar(gyro_orientation), (long)gyro_fsr << 15);
		inv_set_accel_orientation_and_scale(
			inv_orientation_matrix_to_scalar(gyro_orientation), (long)accel_fsr << 15);
		inv_set_compass_orientation_and_scale(
			inv_orientation_matrix_to_scalar(comp_orientation), (long)compass_fsr << 15);

		res = dmp_load_motion_driver_firmware(); //加载dmp固件
		if (res)
			return 6;
		mpl_printf("DMP load OK!\n");
		res = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); //设置陀螺仪方向
		if (res)
			return 7;
		mpl_printf("DMP set orientation OK!\n");
		res = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | //设置dmp功能
								 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
								 DMP_FEATURE_GYRO_CAL);
		if (res)
			return 8;
		mpl_printf("DMP feature set OK!\n");
		res = dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
		if (res)
			return 9;
		mpl_printf("DMP output rate set OK!\n");
		res = run_self_test(); //自检
		if (res)
			return 10;
		mpl_printf("self test OK!\n");
		res = mpu_set_dmp_state(1); //使能DMP
		if (res)
			return 11;
		mpl_printf("DMP enabled\n");
		return 0;
	}
	else
		return 1;
}

/**
 * @brief 获取MPL处理后的数据
 * 
 * @param ptich 俯仰角 精度0.1 范围 -90<-->90
 * @param roll 	横滚角 精度0.1 范围-180<-->180
 * @param yaw 	航向角 精度0.1 范围-180<-->180
 * @return 0则正常，其他则失败
 */
uint8_t MPU9250_MPL_getData(float *pitch, float *roll, float *yaw)
{
	unsigned long sensor_timestamp, timestamp;
	short gyro[3], accel_short[3], compass_short[3], sensors;
	unsigned char more;
	long compass[3], accel[3], quat[4], temperature;
	long data[9];
	int8_t accuracy;

	if (dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more))
		return 1;

	if (sensors & INV_XYZ_GYRO)
	{
		inv_build_gyro(gyro, sensor_timestamp); //把新数据发送给MPL
		mpu_get_temperature(&temperature, &sensor_timestamp);
		inv_build_temp(temperature, sensor_timestamp); //把温度值发给MPL，只有陀螺仪需要温度值
	}

	if (sensors & INV_XYZ_ACCEL)
	{
		accel[0] = (long)accel_short[0];
		accel[1] = (long)accel_short[1];
		accel[2] = (long)accel_short[2];
		inv_build_accel(accel, 0, sensor_timestamp); //把加速度值发给MPL
	}

	if (!mpu_get_compass_reg(compass_short, &sensor_timestamp))
	{
		compass[0] = (long)compass_short[0];
		compass[1] = (long)compass_short[1];
		compass[2] = (long)compass_short[2];
		inv_build_compass(compass, 0, sensor_timestamp); //把磁力计值发给MPL
	}
	inv_execute_on_data();
	inv_get_sensor_type_euler(data, &accuracy, &timestamp);

	*roll = (data[0] / q16);
	*pitch = -(data[1] / q16);
	*yaw = -data[2] / q16;
	return 0;
}

uint8_t MPU_i2cWrite(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t *data)
{
	return HAL_I2C_Mem_Write(&hi2c1, slave_addr << 1, reg_addr, 1, data, length, 0xff);
}

uint8_t MPU_i2cRead(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t *data)
{
	return HAL_I2C_Mem_Read(&hi2c1, slave_addr << 1, reg_addr, 1, data, length, 0xff);
}

void mpl_getms(uint32_t *num)
{
	*num = Get_Systick();
}
void mpl_printf(const char *fmt, ...)
{
	va_list ap;
	va_start(ap, fmt);
	int length;
	char buffer[128];
	length = vsnprintf(buffer, 128, fmt, ap);
	HAL_UART_Transmit(&huart1, (uint8_t *)buffer, length, 0xffff);
}
